/******************************************************************************
 *
 * MotionViewer
 * Copyright (C) 2007 Sangyoon Lee, Electronic Visualization Laboratory, 
 * University of Illinois at Chicago
 *
 * This software is free software; you can redistribute it and/or modify it 
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either Version 2.1 of the License, or 
 * (at your option) any later version.
 *
 * This software is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 
 * License for more details.
 * 
 * You should have received a copy of the GNU Lesser Public License along
 * with this software; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Questions or comments about MotionViewer should be directed to 
 * cavern@evl.uic.edu
 *
 *****************************************************************************/

#pragma warning(disable:4267)	
#pragma warning(disable:4018)	
#include <ogreplatform.h>
#if OGRE_PLATFORM == OGRE_PLATFORM_APPLE
#include <Carbon/Carbon.h>
#endif


#include "ViconMotionDataSet.h"
#include "ViconSubject.h"

void vtxVicon2Ogre(float* v)
{
	// correct coordinate system
	// OGRE is typical Right Hand Opengl coordinate system
	/*
		| +y
		|
		|	
		----------- +x
		/
	   /
	  / +z

	*/

	// Vicon Coordinate system (subject facing to +y)
	/*
	 +z |   /  +y
		|  /
		| /	
		----------- +x
	*/

	// therefore, x = -x, swap y and z
	v[0] = -v[0];
	v[0] = v[0];
	float temp = v[1];		// store y
	v[1] = v[2];			// y' = z
	v[2] = temp;			// z = -y

}

void qtVicon2Ogre(float* q)
{
	q[1] = -q[1];
	float temp = q[2];
	q[2] = q[3];
	q[3] = temp;
}

void rotVicon2Ogre(float* r)
{
	//r[0] = -r[0];
	float temp = r[1];		// store y
	r[1] = r[2];			// y' = z
	r[2] = temp;			// z = -y
}


ViconMotionDataSet::ViconMotionDataSet()
{
	m_tType = MT_NONE;
	m_sDebugBodyInfo = "";
	m_bDebug = false;
	m_pSubject = NULL;

	m_fFrameRate = 120.0f;
	m_fSpeed = 1.0f;
	m_iTotFrames = 0;
	m_iCurrFrame = 0;

	static int g_iMotionID = -1;
	g_iMotionID++;
	char str[64];
	sprintf(str, "MotionData_%i", g_iMotionID);
	m_sName = std::string(str);

}

ViconMotionDataSet::~ViconMotionDataSet()
{
	if (m_pSubject)
		delete m_pSubject;
}

void ViconMotionDataSet::update(float addedTime)
{
	if (m_bDebug)
		updateDebug();
}

void ViconMotionDataSet::connectToSource(const char* source)
{
	m_sSource = std::string(source);
}

void ViconMotionDataSet::updateDebug()
{
}

void ViconMotionDataSet::getMarkerPosition(int id, float* pos)
{
	pos[0] = pos[1] = pos[2] = 0.0f;
}

void ViconMotionDataSet::getBodyTranslation(int id, float* trans, bool global)
{
	trans[0] = trans[1] = trans[2] = 0.0f;
}

void ViconMotionDataSet::getBodyRotation(int id, float* rot, bool global)
{
	rot[0] = rot[1] = rot[2] = 0.0f;
}

void ViconMotionDataSet::getBodyQuaternion(int id, float* qt, bool global)
{
	qt[0] = 1.0f;
	qt[1] = qt[2] = qt[3] = 0.0f;
}

int ViconMotionDataSet::getNumMarkers()
{
	return m_vMarkerChannels.size();
}

int ViconMotionDataSet::getNumBodies()
{
	return m_vBodyChannels.size();
}

std::string  ViconMotionDataSet::getMarkerName(int id)
{
	if (id < 0 || id > m_vMarkerChannels.size() - 1)
		return "";

	return m_vMarkerChannels[id].Name;
}

std::string  ViconMotionDataSet::getBodyName(int id)
{
	if (id < 0 || id > m_vBodyChannels.size() - 1)
		return "";

	return m_vBodyChannels[id].Name;
}

int	ViconMotionDataSet::getBodyIndex(std::string bodyname)
{
	// TODO
	int result = -1;
	std::vector< BodyChannel >::iterator iBody;
	int found = 0;
	for(iBody = m_vBodyChannels.begin(); iBody != m_vBodyChannels.end(); iBody++, found++)
	{
		if(iBody->Name.compare(bodyname) ==0)
		{
			result = found;
			break;
		}
	}

	return result;
}

int ViconMotionDataSet::getMarkerIndex(std::string markername)
{
	// TODO
	int result = -1;
	std::vector< MarkerChannel >::iterator iMarker;
	int found = 0;
	for(iMarker = m_vMarkerChannels.begin(); iMarker != m_vMarkerChannels.end(); iMarker++, found++)
	{
		if(iMarker->Name.compare(markername) ==0)
		{
			result = found;
			break;
		}
	}

	return result;
}

void ViconMotionDataSet::loadSubject(const char* subject, const char* filename)
{
	m_pSubject = new ViconSubject(subject);
	m_pSubject->loadSubjectFile(filename);
}

bool ViconMotionDataSet::exportBVH(const char* filename)
{
	// open file
	std::ifstream v(filename, std::ios::out);
	if (!v) {
		assert("fail to open file");
		return false;
	}
	
	// write hierachy
	if (m_pSubject == NULL)
		return false;
	
	if (!m_pSubject->writeBVHHierachy(v))
		return false;

	// write motion data
	//MOTION
	//Frames: xxxx
	//Frame Time: 0.xxxxx
	//channel data
	
	return true;
}

void ViconMotionDataSet::setSpeed(float speed) 
{
	m_fSpeed = speed;
}

void ViconMotionDataSet::increaseSpeed()
{
	m_fSpeed *= 1.01f;
}

void ViconMotionDataSet::decreaseSpeed()
{
	m_fSpeed *= 0.99f;
}


std::string ViconMotionDataSet::getSubjectName()
{
	if (m_pSubject)
		return m_pSubject->getName();
	else
		return std::string("");
}

std::string ViconMotionDataSet::getSubjectFullPath()
{
	if (m_pSubject)
		return m_pSubject->getFullPathName();
	else
		return std::string("");
}

std::string ViconMotionDataSet::getSubjectFileName()
{
	if (m_pSubject)
		return m_pSubject->getFileName();
	else
		return std::string("");
}
